#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/empty.hpp>
#include <serial/serial.h>  // 引入 serial 库
#include <iomanip>
#include <chrono>
#include <cmath>
#include <std_msgs/msg/float64.hpp>

class EncoderDriver : public rclcpp::Node
{
public:
    EncoderDriver() : Node("encoder_driver"), clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
    {
        RCLCPP_WARN(this->get_logger(), "Starting encoder driver node");

        // 从参数服务器获取 encoder_port_name
        this->declare_parameter<std::string>("encoder_port_name", "/dev/ttyUSB0"); // 默认串口
        this->get_parameter("encoder_port_name", encoder_port_name_);

        // 打开串口
        open_serial_port();

        reset_counter();
        last_rcv_set_zero_time = clock_->now();  // 初始化时间标志
        last_rcv_pulse_time = clock_->now();

        // 定时器
        read_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0 / 10.0), std::bind(&EncoderDriver::timer_callback, this));

        open_serial_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0), std::bind(&EncoderDriver::open_serial_port_timer_callback, this));

        set_zero_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/winch_wireless_server/set_zero", 10, std::bind(&EncoderDriver::setZeroCallback, this, std::placeholders::_1));
        
        length_pub = this->create_publisher<std_msgs::msg::Float64>("/encode_driver/length", 10);
    }

private:
    std::string encoder_port_name_;
    serial::Serial ser;  // 使用 serial 库管理串口
    rclcpp::TimerBase::SharedPtr read_timer_, open_serial_timer_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_trigger_pub;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr set_zero_sub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr length_pub;
    bool last_switch_value = false;
    int pack_length = 9;
    const int pulse_per_round = 100;
    const double radius = 0.015;
    rclcpp::Time last_rcv_set_zero_time;  // 使用 rclcpp::Time 替换 std::chrono
    rclcpp::Time last_rcv_pulse_time;
    bool set_zero_flag = false;
    // rclcpp::Duration set_zero_wait_time = rclcpp::Duration::from_seconds(0.05); // 替换为 rclcpp::Duration
    rclcpp::Duration set_zero_wait_time = rclcpp::Duration::from_seconds(0.01); // 替换为 rclcpp::Duration
    rclcpp::Duration rcv_pulse_timeout = rclcpp::Duration::from_seconds(0.2); // 替换为 rclcpp::Duration

    rclcpp::Clock::SharedPtr clock_;  // 使用共享的 Clock 对象


    void setZeroCallback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        set_zero_flag = true;
        last_rcv_set_zero_time = clock_->now();  // 使用统一时间源
    }

    void open_serial_port()
    {
        try
        {
            // 如果串口已打开，先关闭
            if (ser.isOpen())
            {
                RCLCPP_WARN(this->get_logger(), "Serial port is already open. Closing it first.");
                ser.close();
            }
            ser.setPort(encoder_port_name_);

            RCLCPP_INFO(this->get_logger(), "Encoder opening port: %s", encoder_port_name_.c_str());

            ser.setBaudrate(9600);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);

            ser.open();
        }
        catch (const serial::IOException &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Unable to open port %s", encoder_port_name_.c_str());
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
        }
        ClearSerialBuffer();
    }

    void ClearSerialBuffer()
    {
        RCLCPP_WARN(this->get_logger(), "Data is blocked! Clearing buffer...");
        if (!ser.isOpen())
        {
            std::cerr << "Serial port is not open." << std::endl;
            return;
        }

        std::vector<uint8_t> buffer(ser.available());
        ser.read(buffer, buffer.size());

        RCLCPP_WARN(this->get_logger(), "Cleared buffer data:");
        printHex(buffer);

        // You might also want to write a dummy byte and then flush the output buffer
        ser.flushOutput();
    }

    void printHex(const std::vector<uint8_t> &data)
    {
        std::stringstream ss;
        ss << std::hex << std::setw(2) << std::setfill('0');
        for (std::size_t i = 0; i < data.size(); ++i)
        {
            ss << std::setw(2) << static_cast<int>(data[i]) << ' ';
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }

    void open_serial_port_timer_callback()
    {
        if (!ser.isOpen())
        {
            open_serial_port();
        }
    }

    void decode_tty_data(std::vector<uint8_t> tty_buffer)
    {
        uint8_t id = tty_buffer[0];
        uint8_t control_code = tty_buffer[1];
        uint8_t data_count = tty_buffer[2];

        if (id != 0x01 || control_code != 0x03 || data_count != 0x04)
        {
            RCLCPP_WARN(this->get_logger(), "Data does not match!");
            printHex(tty_buffer);
            return;
        }

        int32_t data = (static_cast<int32_t>(tty_buffer[3]) << 24) |  // 高字节
                       (static_cast<int32_t>(tty_buffer[4]) << 16) |
                       (static_cast<int32_t>(tty_buffer[5]) << 8) |
                       (static_cast<int32_t>(tty_buffer[6]));       // 低字节

        // RCLCPP_INFO(this->get_logger(), "Data: %d", data);

        double length = static_cast<double>(data) / pulse_per_round * 2 * M_PI * radius;
        RCLCPP_INFO(this->get_logger(), "Total length = %f", length);
        std_msgs::msg::Float64 msg;
        msg.data = length;
        length_pub->publish(msg);
        last_rcv_pulse_time = clock_->now();  // 更新时间标志
    }

    void reset_counter()
    {
        std::vector<uint8_t> reset_counter_data = {0x01, 0x10, 0x00, 0x30, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xBB};
        ser.write(reset_counter_data);
        RCLCPP_INFO(this->get_logger(), "Reset counter message sent");
        rclcpp::sleep_for(std::chrono::milliseconds(50));
        size_t bytes_read = 0;
        std::vector<uint8_t> buffer(0);
        int available_length = ser.available();

        if (available_length > 0)
        {
            bytes_read = ser.read(buffer, available_length);
            printHex(buffer);
            RCLCPP_INFO(this->get_logger(), "Reset counter success");
        }
    }

    void timer_callback()
    {
        if (ser.isOpen())
        {
            try
            {
                rclcpp::Time now = clock_->now();  // 获取当前时间
                if((now - last_rcv_pulse_time) > rcv_pulse_timeout)
                {
                    RCLCPP_WARN(this->get_logger(), "No pulse data received for %f seconds", (now - last_rcv_pulse_time).seconds());
                }

                if (set_zero_flag)
                {
                    reset_counter();
                    set_zero_flag = false;
                    return;
                }
                else if ((now - last_rcv_set_zero_time) < set_zero_wait_time)
                {
                    RCLCPP_INFO(this->get_logger(), "Waiting for set_zero command");
                    return;
                }

                std::vector<uint8_t> ask_pulse_data = {0x01, 0x03, 0x00, 0x40, 0x00, 0x02, 0xC5, 0xDF};
                ser.write(ask_pulse_data);

                // 读取串口返回的数据
                std::vector<uint8_t> buffer(0);
                int available_length = ser.available();
                size_t bytes_read = 0;
                if (available_length > 0)
                {
                    bytes_read = ser.read(buffer, available_length);
                }
                else
                {
                    return;
                }

                if (bytes_read == pack_length)
                {
                    decode_tty_data(buffer);
                }
                else
                {
                    RCLCPP_WARN(this->get_logger(), "Read data length error! %d", bytes_read);
                    printHex(buffer);
                    ClearSerialBuffer();
                }
            }
            catch (const serial::SerialException &e) // 捕获串口相关的异常
            {
                RCLCPP_ERROR(this->get_logger(), "SerialException: %s", e.what());
            }
            catch (const std::exception &e) // 捕获通用异常
            {
                RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
            }
            
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<EncoderDriver>());
    rclcpp::shutdown();
    return 0;
}
